/*
 * basic.cpp
 *
 *  Created on: 07.10.2011
 *      Author: ignacio
 */

#include "Character.h"

using neurignacio::sin;
using neurignacio::cos;
using neurignacio::atan2;
using neurignacio::pow;

#include <iostream>
using std::cout;
using std::endl;
#include <sstream>

using std::stringstream;

#include <cmath>

#include <algorithm> // for std::sort

namespace neurignacio{


}// end namespace neurignacio

namespace neurignacio{

ostream& operator<<(ostream& out, const Quark& quark)
{
	out << quark.rho << "@" << quark.theta;
	return out;
}

ostream& operator<<(ostream& out, const SteeringOutput& steering)
{
	out << static_cast<Quark>(steering);
	out << "|" << steering.magnitude() << "|";
	return out;
}

ostream& operator<<(ostream& out, const Lepton& lepton)
{
	out << "L#"<<static_cast<Location>(lepton)<<"#";
	out << ",V#" << lepton.velocity << "|" << lepton.velocity.magnitude() << "|"<<"#";
	out << "R#" << lepton.rotation << "#";
}

void Location::integrate(const SteeringOutput& steer, real duration)
{
	position += steer.linear * duration;
    orientation += steer.angular * duration;
    orientation.normalize();
}

void Location::setOrientationFromVelocity(const Vector3& velocity)
{
    // If we haven't got any velocity, then we can do nothing.
    if (velocity.squareMagnitude() > 0) {
        orientation = angle(std::atan2(velocity.x, velocity.z), radians);
    }
}

Vector3 Location::getOrientationAsVector() const
{
	Vector3 v;
	v.x=std::sin(orientation.to_radians());
	v.y=0;
	v.z=std::cos(orientation.to_radians());
	return v;
 //   return Vector3(sin(orientation), 0, cos(orientation));
}


void Lepton::integrate(real duration)
    {
	stringstream out;
	out << "\t+++++ Integrate(real) - Line " << __LINE__ << endl;
	out << "\torientation: " << orientation << " rotation: " << rotation << " duration: " << duration;
	position += velocity*duration;
	orientation += rotation*duration;
	orientation.normalize_180();
	out << " orientation.normalize_180(): " << orientation << endl;

//	cout << out.str();
    }

void Lepton::integrate(const SteeringOutput& steer, real duration)
{
	stringstream out;
	out << "\t##### Integrate(steer) - Line " << __LINE__ << endl;
	out << "\torientation: " << orientation << " rotation: " << rotation << " duration: " << duration;
	out << "\n\tsteer.angular: "<< steer.angular << " duration: " << duration<< " steer.angular*duration=" << steer.angular*duration <<endl;
	Lepton::integrate(duration);
	velocity += steer.linear*duration;

    rotation += steer.angular*duration;//+= steer.angular*duration; // Original: orientation += steer.angular*duration; // Shouldn't be rotation????

  //  rotation.normalize_180();

    out<< "\t=> rotation after steering: " << rotation << " rotation*duration: "<< rotation*duration << endl;

//    cout << out.str();
}

void Lepton::integrate(const SteeringOutput& steer, real drag, real duration)
{
	integrate(duration);

    // Slowing velocity and rotational velocity
    drag = pow(drag, duration);
    velocity *= drag;
    rotation *= drag*drag;

    velocity += steer.linear*duration;
    rotation += steer.angular*duration;
}

void Lepton::integrate(const SteeringOutput& steer, const SteeringOutput& drag, real duration)
{
	integrate(duration);

	velocity *= pow(drag.linear, duration);
	rotation *= pow(drag.angular, duration);
	velocity += steer.linear*duration;
	rotation += steer.angular*duration;
}

void Lepton::trimMaxSpeed(const real& maxSpeed)
{
	if (velocity.squareMagnitude() > maxSpeed*maxSpeed) {
		velocity.normalize();
		velocity *= maxSpeed;
	}
}

void Lepton::trimMaxRotation(const angle& maxRotation)
{
	if (abs(rotation)>abs(maxRotation))
	{
		rotation=maxRotation*rotation.sign();
	}
}

bool Lepton::operator == (const Lepton& other) const
{
	return position == other.position &&
		   orientation == other.orientation &&
		   velocity == other.velocity &&
		   rotation == other.rotation;
}
bool Lepton::operator!= (const Lepton& other) const
{
	return position != other.position ||
		   orientation != other.orientation ||
		   velocity != other.velocity ||
		   rotation != other.rotation;
}
Lepton& Lepton::operator = (const Location& other)
{
	orientation = other.orientation;
	position = other.position;
	return *this;
}
Lepton& Lepton::operator = (const Lepton& other)
{
	orientation = other.orientation;
	position = other.position;
	velocity = other.velocity;
	rotation = other.rotation;
	return *this;
}
void Lepton::operator += (const Lepton& other)
{
	position+=other.position;
	velocity+=other.velocity;
	rotation+=other.rotation;
	orientation+=other.orientation;
}

void Lepton::operator -= (const Lepton& other)
{
	position-=other.position;
	velocity-=other.velocity;
	rotation-=other.rotation;
	orientation-=other.orientation;
}

void Lepton::operator *= (real f)
{
	position*=f;
	velocity*=f;
	rotation*=f;
	orientation*=f;
}

ostream& operator<<(ostream& out, const Muon& muon)
{
	out << "{" << static_cast<Lepton>(muon) <<"}";
	out << "[V_max=" << muon.maxVelocity;
	out << ",Rot_max=" << muon.maxRotation;
	out << ",V_min=" << muon.minVelocity;
	out << ",Acc_max" << muon.maxAcceleration;
	out << ",AngAcc_max="<<muon.maxAngularAcceleration;
	out << "]";
	return out;
}
} // end namespace neurignacio


